//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>
#include <sysnum.h>
#include <init.h>
#include "exception.h"

#include <asm.h>

#define INTERRUPT_CONTEXT_MARK      0xA5A5B6B6
#define INTERRUPT_CONTEXT_MARKSTR   "0xA5A5B6B6"

const uint_t c_uMaxNumberOfInterrupts = 64;
const uint_t c_uNumberOfIpls          = IPL31 + 1;

typedef struct IsrDescriptor
{
    ipl_t   ipl;
    isr_t   isr;
    void    *pvDevice;
} IsrDescriptor;

EXTERN ipl_t            g_currentIpl;
EXTERN irqs_t           g_irqsTable[c_uNumberOfIpls + 1];
EXTERN irqs_t           g_activeIrqs;
EXTERN IsrDescriptor    g_isrDescriptors[c_uMaxNumberOfInterrupts];

ipl_t           g_currentIpl = IPL_Base;
irqs_t          g_irqsTable[c_uNumberOfIpls + 1];
irqs_t          g_activeIrqs = 0;
IsrDescriptor   g_isrDescriptors[c_uMaxNumberOfInterrupts];

DECL_ASMLINKAGE void RSTExceptionHandler();
DECL_ASMLINKAGE void UNDExceptionHandler();
DECL_ASMLINKAGE void SWIExceptionHandler();
DECL_ASMLINKAGE void PABTExceptionHandler();
DECL_ASMLINKAGE void DABTExceptionHandler();
DECL_ASMLINKAGE void ADRExceptionHandler();
DECL_ASMLINKAGE void IRQExceptionHandler();
DECL_ASMLINKAGE void FIQExceptionHandler();

DECL_ASMLINKAGE void VectorTableBegin();
DECL_ASMLINKAGE void VectorTableEnd();

typedef struct ExceptionStack
{
    uint32_t    lr, cpsr;
} ExceptionStack;

DECL_ASMLINKAGE ExceptionStack g_undStack, g_abtStack, g_irqStack;

ExceptionStack g_undStack, g_abtStack, g_irqStack;

DECL_ASMLINKAGE void CDECL ReportInvalidMode(InterruptContext *pContext);
DECL_ASMLINKAGE void CDECL TryToSchedule();

STATIC DECL_CODEINIT void CDECL InitVectorTable()
{
    const uint_t c_uSizeOfVectorTable =
        (uint32_t)&VectorTableEnd - (uint32_t)&VectorTableBegin;
    assert(c_uMaxSizeOfVectorTable >= c_uSizeOfVectorTable);

    memcpy(g_vectorTableArea, (void *)&VectorTableBegin,
       c_uSizeOfVectorTable);
}

EXTERN DECL_CODEINIT bool_t CDECL InitInterrupts()
{
    InitSystemCallTable();

    InitVectorTable();

    BspMaskAllIrqs(FALSE);

    Sti();

    return TRUE;
}

EXTERN void CDECL DzRegisterIsr(irq_t irq, ipl_t ipl, isr_t isr,
    void *pvDevice)
{
    assert(bsp.uNumberOfIrqs > irq);
    assert(IPL31 >= ipl);
    assert(isr);

    uint32_t uFlags = SaveFlagsAndCli();

    if (NULL != g_isrDescriptors[irq].isr) DzUnregisterIsr(irq);

    g_isrDescriptors[irq].ipl       = ipl;
    g_isrDescriptors[irq].isr       = isr;
    g_isrDescriptors[irq].pvDevice  = pvDevice;

    for (uint_t i = 0; i <= g_isrDescriptors[irq].ipl; i++) {
        g_irqsTable[i] |= (irqs_t)1 << irq;
    }

    DzEnableIrq(irq);

    RestoreIF(uFlags);
}

EXTERN void CDECL DzUnregisterIsr(irq_t irq)
{
    assert(bsp.uNumberOfIrqs > irq);
    assert(g_isrDescriptors[irq].isr);

    uint32_t uFlags = SaveFlagsAndCli();

    DzDisableIrq(irq);

    for (uint_t i = 0; i <= g_isrDescriptors[irq].ipl; i++) {
        g_irqsTable[i] &= ~((irqs_t)1 << irq);
    }

    g_isrDescriptors[irq].isr = NULL;

    RestoreIF(uFlags);
}

EXTERN void CDECL DzEnableIrq(irq_t irq)
{
    assert(bsp.uNumberOfIrqs > irq);

    uint32_t uFlags = SaveFlagsAndCli();
    if (NULL != g_isrDescriptors[irq].isr) {
        g_activeIrqs |= (irqs_t)1 << irq;

        if (g_isrDescriptors[irq].ipl < g_currentIpl) {
            BspMaskIrqs((irqs_t)1 << irq, TRUE);
        }
    }
    RestoreIF(uFlags);
}

EXTERN void CDECL DzDisableIrq(irq_t irq)
{
    assert(bsp.uNumberOfIrqs > irq);

    uint32_t uFlags = SaveFlagsAndCli();
    if (NULL != g_isrDescriptors[irq].isr) {
        if (g_isrDescriptors[irq].ipl < g_currentIpl) {
            BspMaskIrqs((irqs_t)1 << irq, FALSE);
        }

        g_activeIrqs &= ~((irqs_t)1 << irq);
    }
    RestoreIF(uFlags);
}

EXTERN ipl_t CDECL DzRaiseCurrentIpl(ipl_t ipl)
{
    assert(IPL31 >= ipl);

    uint32_t uFlags = SaveFlagsAndCli();

    register ipl_t prevIpl = g_currentIpl;

    if (ipl < g_currentIpl) {
        BspMaskIrqs(
            (g_irqsTable[ipl] - g_irqsTable[g_currentIpl]) & g_activeIrqs,
            FALSE);
        g_currentIpl = ipl;
    }

    RestoreIF(uFlags);

    return prevIpl;
}

EXTERN ipl_t CDECL DzLowerCurrentIpl(ipl_t ipl)
{
    assert(IPL1 <= ipl && ipl <= IPL_Base);

    uint32_t uFlags = SaveFlagsAndCli();

    register ipl_t prevIpl = g_currentIpl;

    if (ipl > g_currentIpl) {
        BspMaskIrqs(
            (g_irqsTable[g_currentIpl] - g_irqsTable[ipl]) & g_activeIrqs,
            TRUE);
        g_currentIpl = ipl;
    }

    RestoreIF(uFlags);

    return prevIpl;
}

DECL_ASMLINKAGE void CDECL UpdateTime(InterruptContext *pContext);

STATIC void CDECL InvokeIsr(irq_t irq, InterruptContext *pContext);

STATIC uint_t s_uInterruptNested = 0;

EXTERN bool_t CDECL DzIsInIsr()
{
    return s_uInterruptNested > 0;
}

INLINE void volatile CheckInterruptMark(InterruptContext *pContext)
{
    if (*((unsigned int*)pContext - 1) != INTERRUPT_CONTEXT_MARK) {
        kprintf("Interrupt Context Crashed!\n");
        DebugBreak();
    }
}

DECL_ASMLINKAGE void CDECL HandleInterrupt(InterruptContext *pContext)
{
    register irqs_t irqs = BspGetIrqs() & g_activeIrqs;

    CheckInterruptMark(pContext);

    if (0 == irqs) {
        // bugbug: for some unknown reasons, it will happen in Gingko opt!
        // assert("inactive irqs" && 0);
        return;
    }

    ipl_t iplHigh = IPL_Base;
    irq_t irqHigh = bsp.uNumberOfIrqs;

    for (irq_t irq = 0; irq < bsp.uNumberOfIrqs; irq++) {
        if (0 == (irqs & ((irqs_t)1 << irq))) continue;

        if (g_isrDescriptors[irq].ipl < iplHigh) {
            iplHigh = g_isrDescriptors[irq].ipl;
            irqHigh = irq;
        }
    }

    assert(irqHigh < bsp.uNumberOfIrqs);

    bool_t bOriginalPreemption = DzDisablePreemption();

    CheckInterruptMark(pContext);

    InvokeIsr(irqHigh, pContext);

    CheckInterruptMark(pContext);

    RestorePreemptionByIsr(bOriginalPreemption);

    if (0 == s_uInterruptNested && 0 < g_uLostTicks) {
        UpdateTime(pContext);
    }

    CheckInterruptMark(pContext);
}

STATIC void CDECL InvokeIsr(irq_t irq, InterruptContext *pContext)
{
    assert(g_isrDescriptors[irq].isr);

    register ipl_t prevIpl = g_currentIpl;

    assert(g_isrDescriptors[irq].ipl < g_currentIpl);

    g_currentIpl = g_isrDescriptors[irq].ipl;
    BspMaskIrqs(
        (g_irqsTable[g_currentIpl] - g_irqsTable[prevIpl]) & g_activeIrqs,
        FALSE);

    BspAcknowledgeIrq(irq);
    s_uInterruptNested++;
    Sti();

    (*g_isrDescriptors[irq].isr)(
            irq, g_isrDescriptors[irq].pvDevice, pContext);

    Sti();

    // TODO: need optimize?
    Cli();
    s_uInterruptNested--;
    BspMaskIrqs(
        (g_irqsTable[g_currentIpl] - g_irqsTable[prevIpl]) & g_activeIrqs,
        TRUE);
    g_currentIpl = prevIpl;
    Sti();
}

DECL_ASMLINKAGE void CDECL TryToSchedule()
{
    if (PreemptionEnabled() &&
        InterlockedExchange(&GetCurrentThread()->m_bNeedReschedule, FALSE)) {
        Schedule();
    }
}

DECL_ASMLINKAGE void CDECL DoRSTException()
{
    kprintf("System reset ...\n");

    BspReboot();
}

DECL_ASMLINKAGE void CDECL DoADRException()
{
    // It is supposed never happen.
    kprintf("*ERROR* Address exception\n");

    BspHalt();
}

DECL_ASMLINKAGE void CDECL StiAndEnablePreemption()
{
    Sti();

//    TIMEPEG_SCHEDULE_STOP

    EnablePreemption();
}

DECL_ASMLINKAGE void CDECL ReportInvalidInstruction()
{
    kprintf("*ERROR* Invalid instruction when cause data abort\n");
}

DECL_ASMLINKAGE void CDECL ReportInvalidMode(InterruptContext *pContext)
{
    kprintf("*ERROR* Detected invalid exception mode\n");

    BspHalt();
}

#if defined(_GNUC)

EXTERN void CDECL ExceptionHandlers()
{
#define SET_EXCEPTION_HANDLER(handler) \
    ASM("ldr    pc, AddressOf"#handler";")

#define DEFINE_EXCEPTION_ADDRESS(handler) \
    ASM("AddressOf"#handler": .long "C_PREFIX#handler"\n")

DECL_ASMENTRY(VectorTableBegin)

    SET_EXCEPTION_HANDLER(RSTExceptionHandler);
    SET_EXCEPTION_HANDLER(UNDExceptionHandler);
    SET_EXCEPTION_HANDLER(SWIExceptionHandler);
    SET_EXCEPTION_HANDLER(PABTExceptionHandler);
    SET_EXCEPTION_HANDLER(DABTExceptionHandler);
    SET_EXCEPTION_HANDLER(ADRExceptionHandler);
    SET_EXCEPTION_HANDLER(IRQExceptionHandler);
    SET_EXCEPTION_HANDLER(FIQExceptionHandler);

    DEFINE_EXCEPTION_ADDRESS(RSTExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(UNDExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(SWIExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(PABTExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(DABTExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(ADRExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(IRQExceptionHandler);
    DEFINE_EXCEPTION_ADDRESS(FIQExceptionHandler);

DECL_ASMENTRY(VectorTableEnd)

    ASM(".align 5;"
        "undStack: .long      "C_PREFIX"g_undStack;"
        "abtStack: .long      "C_PREFIX"g_abtStack;"
        "irqStack: .long      "C_PREFIX"g_irqStack;");

DECL_ASMENTRY(RSTExceptionHandler)

    kprintf("System reset ...\n");

    BspReboot();

DECL_ASMENTRY(UNDExceptionHandler)

    ASM(".align 5;"
        "ldr    sp, undStack;"
        "str    lr, [sp];"          // save lr
        "mrs    lr, spsr;"
        "str    lr, [sp, #4];");    // save spsr

    ASM("mov    sp, %0;"
        "msr    spsr, sp;"          // switch to svc mode
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("and    lr, lr, #0xf;"
        "ldr    lr, [pc, lr, lsl #2];"
        "movs   pc, lr;");          // change mode and branch

    ASM("UNDDispatchTable:"
        ".long  "C_PREFIX"UNDFromUSR;"       // 0
        ".long  "C_PREFIX"UNDFromINV;"       // 1
        ".long  "C_PREFIX"UNDFromINV;"       // 2
        ".long  "C_PREFIX"UNDFromSVC;"       // 3
        ".long  "C_PREFIX"UNDFromINV;"       // 4
        ".long  "C_PREFIX"UNDFromINV;"       // 5
        ".long  "C_PREFIX"UNDFromINV;"       // 6
        ".long  "C_PREFIX"UNDFromINV;"       // 7
        ".long  "C_PREFIX"UNDFromINV;"       // 8
        ".long  "C_PREFIX"UNDFromINV;"       // 9
        ".long  "C_PREFIX"UNDFromINV;"       // 10
        ".long  "C_PREFIX"UNDFromINV;"       // 11
        ".long  "C_PREFIX"UNDFromINV;"       // 12
        ".long  "C_PREFIX"UNDFromINV;"       // 13
        ".long  "C_PREFIX"UNDFromINV;"       // 14
        ".long  "C_PREFIX"UNDFromINV;");     // 15

DECL_ASMENTRY(SWIExceptionHandler)

    ASM(".align 5;"
        "and    r5, r5, #0x3;"
        "ldr    r5, [pc, r5, lsl #2];"
        "mov    pc, r5;");

    ASM("FastSystemCallDispatchTable:"
        ".long  "C_PREFIX"NormalSystemCall;"         // 0
        ".long  "C_PREFIX"FastSystemCall_SwpAdd;"    // 1
        ".long  "C_PREFIX"FastSystemCall_CmpSwp;"    // 2
        ".long  "C_PREFIX"NormalSystemCall;");       // 3

DECL_ASMENTRY(FastSystemCall_SwpAdd)

    ASM(".align 5;"
        "cmp    r0, %0;"
        "ldrlo  r2, [r0];"
        "addlo  r2, r2, r1;"
        "swplo  r2, r2, [r0];"
        "movlo  r0, r2;"
        "movs   pc, lr;"
        : :"i"(KERNEL_BASE));

DECL_ASMENTRY(FastSystemCall_CmpSwp)

    ASM(".align 5;"
        "cmp    r0, %0;"
        "bhs    FastSystemCall_CmpSwp_Exit;"
        "ldr    r3, [r0];"
        "cmp    r3, r2;"
        "streq  r1, [r0];"
        "mov    r0, r3;"
        "FastSystemCall_CmpSwp_Exit:"
        "movs   pc, lr;"
        : :"i"(KERNEL_BASE));

DECL_ASMENTRY(NormalSystemCall)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "add    r8, sp, %1;"
        "stmdb  r8, {sp, lr}^;"     // save sp, lr
        "str    lr, [sp, %1];"      // save pc
        "mrs    r8, spsr;"
        "str    r8, [sp, %2];"      // save cpsr
        "mov    fp, #0;"
        : :"i"(sizeof(InterruptContext)),
        "i"(offsetof(InterruptContext, pc)),
        "i"(offsetof(InterruptContext, cpsr)));

    ASM("bic    lr, lr, #0;"
        "ldr    r7, [lr, #-4];"
        "and    r7, r7, #0x00ffffff;"); // get system call no

    ASM("mov    r8, %0;"
        "msr    cpsr_c, r8;"            // enable irq
        : :"i"(PSR_MODE_SVC));

    ASM("cmp    r7, %0;"
        "ldrcs  r0, InvalidArgument;"   // set r0 = E_INVALID_ARGUMENT
        "bcs    "C_PREFIX"ReturnFromSystemCall;"
        : :"i"(c_uNumOfSystemCall));

    ASM("str    r4, [sp, #-4]!;"        // push arg5
        "adr    lr, SWIExceptionHandler_Next;"
        "ldr    r5, SystemCallTable;"
        "ldr    pc, [r5, r7, lsl #2];"
        "SWIExceptionHandler_Next:"
        "add    sp, sp, #4;"
        "str    r0, [sp, %0];"
        : :"i"(offsetof(InterruptContext, r0)));

    ASM("b      "C_PREFIX"ReturnFromSystemCall;");

    ASM(".align 5;"
        "SystemCallTable: .long "C_PREFIX"g_fnSystemCalls;"
        "InvalidArgument: .long 0x80070057;");

DECL_ASMENTRY(PABTExceptionHandler)

    ASM(".align 5;"
        "ldr    sp, abtStack;"
        "sub    lr, lr, #4;"
        "str    lr, [sp];"          // save lr
        "mrs    lr, spsr;"
        "str    lr, [sp, #4];");    // save spsr

    ASM("mov    sp, %0;"
        "msr    spsr, sp;"          // switch to svc mode
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("and    lr, lr, #0xf;"
        "ldr    lr, [pc, lr, lsl #2];"
        "movs   pc, lr;");          // change mode and branch

    ASM("PABTDispatchTable:"
        ".long  "C_PREFIX"PABTFromUSR;"      // 0
        ".long  "C_PREFIX"PABTFromINV;"      // 1
        ".long  "C_PREFIX"PABTFromINV;"      // 2
        ".long  "C_PREFIX"PABTFromSVC;"      // 3
        ".long  "C_PREFIX"PABTFromINV;"      // 4
        ".long  "C_PREFIX"PABTFromINV;"      // 5
        ".long  "C_PREFIX"PABTFromINV;"      // 6
        ".long  "C_PREFIX"PABTFromINV;"      // 7
        ".long  "C_PREFIX"PABTFromINV;"      // 8
        ".long  "C_PREFIX"PABTFromINV;"      // 9
        ".long  "C_PREFIX"PABTFromINV;"      // 10
        ".long  "C_PREFIX"PABTFromINV;"      // 11
        ".long  "C_PREFIX"PABTFromINV;"      // 12
        ".long  "C_PREFIX"PABTFromINV;"      // 13
        ".long  "C_PREFIX"PABTFromINV;"      // 14
        ".long  "C_PREFIX"PABTFromINV;");    // 15

DECL_ASMENTRY(DABTExceptionHandler)

    ASM(".align 5;"
        "ldr    sp, abtStack;"
        "sub    lr, lr, #8;"
        "str    lr, [sp];"          // save lr
        "mrs    lr, spsr;"
        "str    lr, [sp, #4];");    // save spsr

    ASM("mov    sp, %0;"
        "msr    spsr, sp;"          // switch to svc mode
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("and    lr, lr, #0xf;"
        "ldr    lr, [pc, lr, lsl #2];"
        "movs   pc, lr;");          // change mode and branch

    ASM("DABTDispatchTable:"
        ".long  "C_PREFIX"DABTFromUSR;"      // 0
        ".long  "C_PREFIX"DABTFromINV;"      // 1
        ".long  "C_PREFIX"DABTFromINV;"      // 2
        ".long  "C_PREFIX"DABTFromSVC;"      // 3
        ".long  "C_PREFIX"DABTFromINV;"      // 4
        ".long  "C_PREFIX"DABTFromINV;"      // 5
        ".long  "C_PREFIX"DABTFromINV;"      // 6
        ".long  "C_PREFIX"DABTFromINV;"      // 7
        ".long  "C_PREFIX"DABTFromINV;"      // 8
        ".long  "C_PREFIX"DABTFromINV;"      // 9
        ".long  "C_PREFIX"DABTFromINV;"      // 10
        ".long  "C_PREFIX"DABTFromINV;"      // 11
        ".long  "C_PREFIX"DABTFromINV;"      // 12
        ".long  "C_PREFIX"DABTFromINV;"      // 13
        ".long  "C_PREFIX"DABTFromINV;"      // 14
        ".long  "C_PREFIX"DABTFromINV;");    // 15

DECL_ASMENTRY(ADRExceptionHandler)

    // It is supposed never happen.
    kprintf("*ERROR* Address exception\n");

    BspHalt();

DECL_ASMENTRY(IRQExceptionHandler)
    ASM(".align 5;"
        "ldr    sp, irqStack;"
        "sub    lr, lr, #4;"
        "str    lr, [sp];"          // save lr
        "mrs    lr, spsr;"
        "str    lr, [sp, #4];");    // save spsr

    ASM("mov    sp, %0;"
        "msr    spsr, sp;"          // switch to svc mode
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("and    lr, lr, #0xf;"
        "ldr    lr, [pc, lr, lsl #2];"
        "movs   pc, lr;");          // change mode and branch

    ASM("IRQDispatchTable:"
        ".long  "C_PREFIX"IRQFromUSR;"       // 0
        ".long  "C_PREFIX"IRQFromINV;"       // 1
        ".long  "C_PREFIX"IRQFromINV;"       // 2
        ".long  "C_PREFIX"IRQFromSVC;"       // 3
        ".long  "C_PREFIX"IRQFromINV;"       // 4
        ".long  "C_PREFIX"IRQFromINV;"       // 5
        ".long  "C_PREFIX"IRQFromINV;"       // 6
        ".long  "C_PREFIX"IRQFromINV;"       // 7
        ".long  "C_PREFIX"IRQFromINV;"       // 8
        ".long  "C_PREFIX"IRQFromINV;"       // 9
        ".long  "C_PREFIX"IRQFromINV;"       // 10
        ".long  "C_PREFIX"IRQFromINV;"       // 11
        ".long  "C_PREFIX"IRQFromINV;"       // 12
        ".long  "C_PREFIX"IRQFromINV;"       // 13
        ".long  "C_PREFIX"IRQFromINV;"       // 14
        ".long  "C_PREFIX"IRQFromINV;");     // 15

DECL_ASMENTRY(FIQExceptionHandler)

    ASM(".align 5;"
        "ldr    sp, irqStack;"
        "sub    lr, lr, #4;"
        "str    lr, [sp];"          // save lr
        "mrs    lr, spsr;"
        "str    lr, [sp, #4];");    // save spsr

    ASM("mov    sp, %0;"
        "msr    spsr, sp;"          // switch to svc mode
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("and    lr, lr, #0xf;"
        "ldr    lr, [pc, lr, lsl #2];"
        "movs   pc, lr;");          // change mode and branch

    ASM("FIQDispatchTable:"
        ".long  "C_PREFIX"IRQFromUSR;"       // 0
        ".long  "C_PREFIX"IRQFromINV;"       // 1
        ".long  "C_PREFIX"IRQFromSVC;"       // 2
        ".long  "C_PREFIX"IRQFromSVC;"       // 3
        ".long  "C_PREFIX"IRQFromINV;"       // 4
        ".long  "C_PREFIX"IRQFromINV;"       // 5
        ".long  "C_PREFIX"IRQFromINV;"       // 6
        ".long  "C_PREFIX"IRQFromINV;"       // 7
        ".long  "C_PREFIX"IRQFromINV;"       // 8
        ".long  "C_PREFIX"IRQFromINV;"       // 9
        ".long  "C_PREFIX"IRQFromINV;"       // 10
        ".long  "C_PREFIX"IRQFromINV;"       // 11
        ".long  "C_PREFIX"IRQFromINV;"       // 12
        ".long  "C_PREFIX"IRQFromINV;"       // 13
        ".long  "C_PREFIX"IRQFromINV;"       // 14
        ".long  "C_PREFIX"IRQFromINV;");     // 15

DECL_ASMENTRY(PABTFromINV)
DECL_ASMENTRY(DABTFromINV)

    ASM("sub    sp, sp, %0;"
        "stmia  sp, {r0 - lr};"     // save r0 - lr
        "ldr    r4, abtStack;"
        "b      "C_PREFIX"CallReportInvalidMode;"
        : :"i"(sizeof(InterruptContext)));

DECL_ASMENTRY(IRQFromINV)

    ASM("sub    sp, sp, %0;"
        "stmia  sp, {r0 - lr};"     // save r0 - lr
        "ldr    r4, irqStack;"
        "b      "C_PREFIX"CallReportInvalidMode;"
        : :"i"(sizeof(InterruptContext)));

DECL_ASMENTRY(UNDFromINV)

    ASM("sub    sp, sp, %0;"
        "stmia  sp, {r0 - lr};"     // save r0 - lr
        "ldr    r4, undStack;"
        : :"i"(sizeof(InterruptContext)));

DECL_ASMENTRY(CallReportInvalidMode)

    ASM("mov    fp, #0;"
        "ldmia  r4, {r5 - r6};"     // get pc, cpsr
        "add    r4, sp, %0;"
        "stmia  r4, {r5 - r6};"     // save pc, cpsr
        : :"i"(offsetof(InterruptContext, pc)));

    ASM("mov    r0, sp;"
        "bl     "C_PREFIX"ReportInvalidMode;");

DECL_ASMENTRY(UNDFromUSR)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "ldr    r4, undStack;"
        "add    r8, sp, %1;"
        "ldmia  r4, {r5 - r6};"     // get saved pc, cpsr
        "stmia  r8, {r5 - r6};"     // save pc, cpsr
        "stmdb  r8, {sp, lr}^;"     // save sp_usr, lr_usr
        "mov    fp, #0;"
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, pc)));

    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq
        : :"i"(PSR_MODE_SVC));

    ASM("mov    r0, %0;"
        "mov    r1, $0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromUSRException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_UndefinedInstruction));

DECL_ASMENTRY(PABTFromUSR)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "ldr    r4, abtStack;"
        "add    r8, sp, %1;"
        "ldmia  r4, {r5 - r6};"     // get saved pc, cpsr
        "stmia  r8, {r5 - r6};"     // save pc, cpsr
        "stmdb  r8, {sp, lr}^;"     // save sp_usr, lr_usr
        "mov    fp, #0;"
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, pc)));

    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq
        : :"i"(PSR_MODE_SVC));

    ASM("mov    r0, %0;"
        "mov    r1, $0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromUSRException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_PrefetchAbort));

DECL_ASMENTRY(DataAbort)
// In:  r0 = address of aborted instruction
// Out: r1 != 0 if writing

    ASM(".align 5;"
        "ldr    r4, [r0];"          // Read the aborted instruction
        "tst    r4, r4, lsr #21;"   // C = bit 20
        "sbc    r1, r1, r1;");      // r1 = C - 1

#if defined(_arm720)

    ASM("and    r2, r4, #0xf << 24;"
        "add    pc, pc, r2, lsr #22;"
        "nop;");

    ASM("b      "C_PREFIX"DataAbort_LdrStrSH_Post;"      // 0b0000
        "b      "C_PREFIX"DataAbort_LdrStrSH_Pre;"       // 0b0001
        "b      "C_PREFIX"DataAbort_Invalid;"            // 0b0010
        "b      "C_PREFIX"DataAbort_Invalid;"            // 0b0011
        "b      "C_PREFIX"DataAbort_LdrStr_Offset_Post;" // 0b0100
        "b      "C_PREFIX"DataAbort_LdrStr_Offset_Pre;"  // 0b0101
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Post;"     // 0b0110
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Pre;"      // 0b0111
        "b      "C_PREFIX"DataAbort_LdmStm;"             // 0b1000
        "b      "C_PREFIX"DataAbort_LdmStm;"             // 0b1001
        "b      "C_PREFIX"DataAbort_Invalid;"            // 0b1010
        "b      "C_PREFIX"DataAbort_Invalid;"            // 0b1011
        "b      "C_PREFIX"DataAbort_Normal;"             // 0b1100
        "b      "C_PREFIX"DataAbort_Normal;"             // 0b1101
        "b      "C_PREFIX"DataAbort_Invalid;");          // 0b1111

DECL_ASMENTRY(DataAbort_LdrStrSH_Pre)

    ASM("tst    r4, #1 << 21;"          // Check the W bit
        "beq    "C_PREFIX"DataAbort_Normal;");

DECL_ASMENTRY(DataAbort_LdrStrSH_Post)

    ASM("and    r5, r4, #0x00f;"        // Get Imm[3:0] / Rm
        "tst    r4, #1 << 22;"          // Check the # bit
        "andne  r2, r4, #0xf00;"        // Get Imm[7:4] and combine Imm[3:0]
        "orrne  r2, r5, r2, lsr #4;"
        "ldreq  r2, [sp, r5, lsl #2];"  // Or load Rm value
        "and    r5, r4, #0xf << 16;"    // Get Rn
        "ldr    r0, [sp, r5, lsr #14];" // load Rn value
        "tst    r4, #1 << 23;"          // Check the U bit
        "subne  r7, r0, r2;"
        "addeq  r7, r0, r2;"
        "b      "C_PREFIX"DataAbort_SaveR7;");

DECL_ASMENTRY(DataAbort_LdrStr_Offset_Pre)

    ASM("tst    r4, #1 << 21;"          // Check the W bit
        "beq    "C_PREFIX"DataAbort_Normal;");

DECL_ASMENTRY(DataAbort_LdrStr_Offset_Post)

    ASM("movs   r2, r4, lsl #20;"       // Get offset
        "beq    "C_PREFIX"DataAbort_Normal;"
        "and    r5, r4, #0xf << 16;"    // Get Rn
        "ldr    r0, [sp, r5, lsr #14];"
        "tst    r4, #1 << 23;"          // Check the U bit
        "subne  r7, r0, r2, lsr #20;"
        "addeq  r7, r0, r2, lsr #20;"
        "b      "C_PREFIX"DataAbort_SaveR7;");

DECL_ASMENTRY(DataAbort_LdrStr_Rm_Pre)

    ASM("tst    r4, #1 << 21;"          // Check the W bit
        "beq    "C_PREFIX"DataAbort_Normal;");

DECL_ASMENTRY(DataAbort_LdrStr_Rm_Post)

    ASM("and    r5, r4, #0xf;"
        "ldr    r2, [sp, r5, lsl #2];"  // Get Rm
        "mov    r3, r4, lsr #7;"
        "ands   r3, r3, #0x1f;"         // Get ShiftImm
        "and    r6, r4, #0x70;"
        "orreq  r6, r6, #8;"
        "add    pc, pc, r6;"
        "nop;");

    ASM("mov    r2, r2, lsl r3;"            // 0: LSL(0) && ShiftImm != 0
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 1: LSL(0) && ShiftImm == 0
        "nop;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 2: ???
        "nop;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 3: ???
        "nop;");

    ASM("mov    r2, r2, lsr r3;"            // 4: LSR(1) && ShiftImm != 0
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("mov    r2, r2, lsr #32;"           // 5: LSR(1) && ShiftImm == 32
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 6: ???
        "nop;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 7: ???
        "nop;");

    ASM("mov    r2, r2, asr r3;"            // 8: ASR(2) && ShiftImm != 0
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("mov    r2, r2, asr #32;"           // 9: ASR(2) && ShiftImm == 32
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 10: ???
        "nop;");

    ASM("b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;" // 11: ???
        "nop;");

    ASM("mov    r2, r2, ror r3;"            // 12: ROR(3) && ShiftImm != 0
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("mov    r2, r2, rrx;"               // 13: RRX(3 && ShiftImm == 0)
        "b      "C_PREFIX"DataAbort_LdrStr_Rm_Next;");

    ASM("nop;"                              // 14: ???
        "nop;");

    ASM("nop");                             // 15: ???

    ASM(C_PREFIX"DataAbort_LdrStr_Rm_Next:"
        "and    r5, r4, #0xf << 16;"    // Get Rn
        "ldr    r0, [sp, r5, lsr #14];"
        "tst    r4, #1 << 23;"          // Check the U bit
        "subne  r7, r0, r2;"
        "addeq  r7, r0, r2;"
        "b      "C_PREFIX"DataAbort_SaveR7;");

DECL_ASMENTRY(DataAbort_LdmStm)

    ASM("tst    r4, #1 << 21;"          // Check the W bit
        "beq    "C_PREFIX"DataAbort_Normal;");

    ASM("mov    r7, #0x11;"
        "orr    r7, r7, r7, lsl #8;"
        "and    r0, r4, r7;"            // Get register list
        "and    r2, r4, r7, lsl #1;"
        "add    r0, r0, r2, lsr #1;"
        "and    r2, r4, r7, lsl #2;"
        "add    r0, r0, r2, lsr #2;"
        "and    r2, r4, r7, lsl #3;"
        "add    r0, r0, r2, lsr #3;"
        "add    r0, r0, r0, lsr #8;"
        "add    r0, r0, r0, lsr #4;"
        "and    r7, r0, #0xf;"          // r7 = No of registers to transfer
        "and    r5, r4, #0xf << 16;"    // Get Rn
        "ldr    r0, [sp, r5, lsr #14];"
        "tst    r4, #1 << 23;"          // Check the U bit
        "subne  r7, r0, r7, lsl #2;"
        "addeq  r7, r0, r7, lsl #2;");

DECL_ASMENTRY(DataAbort_SaveR7)

    ASM("str    r7, [sp, r5, lsr #14];");    // Put R7

DECL_ASMENTRY(DataAbort_Normal)

    ASM("mov    pc, lr;");

DECL_ASMENTRY(DataAbort_Invalid)

    kprintf("*ERROR* Invalid instruction when cause data abort\n");

    ASM("mov    pc, lr;");
#elif defined(_arm920) || defined(_xscale) || defined(_arm926) || defined(_arm11)

    ASM("mov    pc, lr;");

#else
#error Unknown architecture variant
#endif

DECL_ASMENTRY(DABTFromUSR)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "ldr    r4, abtStack;"
        "add    r8, sp, %1;"
        "ldmia  r4, {r5 - r6};"     // get saved pc, cpsr
        "stmia  r8, {r5 - r6};"     // save pc, cpsr
        "stmdb  r8, {sp, lr}^;"     // save sp_usr, lr_usr
        "mov    fp, #0;"
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, pc)));

    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq
        : :"i"(PSR_MODE_SVC));

    ASM("mov    r0, r5;"
        "bl     "C_PREFIX"DataAbort;");

    ASM("mov    r0, %0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromUSRException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_DataAbort));

DECL_ASMENTRY(IRQFromUSR)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "ldr    r4, irqStack;"
        "add    r8, sp, %1;"
        "ldmia  r4, {r5 - r6};"     // get saved pc, cpsr
        "stmia  r8, {r5 - r6};"     // save pc, cpsr
        "stmdb  r8, {sp, lr}^;"     // save sp_usr, lr_usr
        "mov    fp, #0;"
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, pc)));

    ASM("ldr    r0, ="INTERRUPT_CONTEXT_MARKSTR";\n"
        "str    r0, [sp, #-4]!;\n"
        "add    r0, sp, #4;\n"
        "bl     "C_PREFIX"HandleInterrupt;\n"
        "ldr    r0, [sp], #4;\n"
        "ldr    r1, ="INTERRUPT_CONTEXT_MARKSTR";\n"
        "cmp    r0, r1;\n"
        ".long  0x17fddefe;\n");

DECL_ASMENTRY(ReturnFromSystemCall)

DECL_ASMENTRY(ReturnFromUSRException)

DECL_ASMENTRY(ReturnFromUSRInterrupt)

    TryToSchedule();

DECL_ASMENTRY(RestoreUSRContext)

    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"        // disable irq
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("ldr    r0, [sp, %0];"
        "msr    spsr, r0;"          // restore cpsr
        "ldmia  sp, {r0 - lr}^;"    // restore r0 - lr
        "mov    r0, r0;"
        "ldr    lr, [sp, %1];"      // get pc
        "add    sp, sp, %2;"        // pop the InterruptContext
        "movs   pc, lr;"            // restore pc and cpsr
        : :"i"(offsetof(InterruptContext, cpsr)),
        "i"(offsetof(InterruptContext, pc)),
        "i"(sizeof(InterruptContext)));

DECL_ASMENTRY(UNDFromSVC)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "add    r5, sp, %0;"
        "mov    r6, lr;"
        "ldr    r7, undStack;"
        "ldmia  r7, {r7 - r8};"
        "add    r4, sp, %1;"
        "stmia  r4, {r5 - r8};"     // save sp_svc, lr_svc, pc, cpsr
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, sp)));

    ASM("mrs    r0, cpsr;"
        "tst    r8, %0;"
        "biceq  r0, r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq if it was
        : :"i"(PSR_I | PSR_F));

    ASM("mov    r0, %0;"
        "mov    r1, $0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromSVCException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_UndefinedInstruction));

DECL_ASMENTRY(PABTFromSVC)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "add    r5, sp, %0;"
        "mov    r6, lr;"
        "ldr    r7, abtStack;"
        "ldmia  r7, {r7 - r8};"
        "add    r4, sp, %1;"
        "stmia  r4, {r5 - r8};"     // save sp_svc, lr_svc, pc, cpsr
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, sp)));

    ASM("mrs    r0, cpsr;"
        "tst    r8, %0;"
        "biceq  r0, r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq if it was
        : :"i"(PSR_I | PSR_F));

    ASM("mov    r0, %0;"
        "mov    r1, $0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromSVCException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_PrefetchAbort));

DECL_ASMENTRY(DABTFromSVC)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "add    r5, sp, %0;"
        "mov    r6, lr;"
        "ldr    r7, abtStack;"
        "ldmia  r7, {r7 - r8};"
        "add    r4, sp, %1;"
        "stmia  r4, {r5 - r8};"     // save sp_svc, lr_svc, pc, cpsr
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, sp)));

    ASM("mrs    r0, cpsr;"
        "tst    r8, %0;"
        "biceq  r0, r0, %0;"
        "msr    cpsr_c, r0;"        // enable irq if it was
        : :"i"(PSR_I | PSR_F));

    ASM("mov    r0, r7;"
        "bl     "C_PREFIX"DataAbort;");

    ASM("mov    r0, %0;"
        "mov    r2, sp;"
        "adr    lr, "C_PREFIX"ReturnFromSVCException;"
        "b      "C_PREFIX"HandleException;"
        : :"i"(ExceptionVector_DataAbort));

DECL_ASMENTRY(IRQFromSVC)

    ASM(".align 5;"
        "sub    sp, sp, %0;"
        "stmia  sp, {r0 - r12};"    // save r0 - r12
        "add    r5, sp, %0;"
        "mov    r6, lr;"
        "ldr    r7, irqStack;"
        "ldmia  r7, {r7 - r8};"
        "add    r4, sp, %1;"
        "stmia  r4, {r5 - r8};"     // save sp_svc, lr_svc, pc, cpsr
        : :"i"(sizeof(InterruptContext)), "i"(offsetof(InterruptContext, sp)));

    ASM("ldr    r0, ="INTERRUPT_CONTEXT_MARKSTR";\n"
        "str    r0, [sp, #-4]!;\n"
        "add    r0, sp, #4;\n"
        "bl     "C_PREFIX"HandleInterrupt;\n"
        "ldr    r0, [sp], #4;\n"
        "ldr    r1, ="INTERRUPT_CONTEXT_MARKSTR";\n"
        "cmp    r0, r1;\n"
        ".long  0x17fddefe;\n");

DECL_ASMENTRY(ReturnFromSVCException)

DECL_ASMENTRY(ReturnFromSVCInterrupt)

    TryToSchedule();

DECL_ASMENTRY(RestoreSVCContext)

    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"        // disable irq
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC));

    ASM("ldr    r0, [sp, %0];"
        "msr    spsr, r0;"
        "ldmia  sp, {r0 - pc}^;"
        : :"i"(offsetof(InterruptContext, cpsr)));

DECL_ASMENTRY(SystemThreadStartup)

    Sti();

//    TIMEPEG_SCHEDULE_STOP

    EnablePreemption();

    ASM("mov    r0, r4;"
        "mov    r1, r5;"
        "mov    pc, r6;");

DECL_ASMENTRY(ThreadStartup)

    Sti();

//    TIMEPEG_SCHEDULE_STOP

    EnablePreemption();

    ASM("mov    r0, r5;"
        "adr    lr, "C_PREFIX"ReturnFromSystemCall;"
        "mov    pc, r4;");
}

#endif // defined(_GNUC)
